#include "SimpleSerial.cpp"
#include <string>
#include <iostream>
#include <time.h>
using namespace std;

/***********************************************************
 *Arduino Commands:
 *
 *These are the commands which the arduino will execute.
 *The pattern to construct a command is:
 *      AACCCCDDDD;
 *Where AA is the instruction identifier, CCCC is the first
 *argument in the instruction, and DDDD is the second
 *argument in the instruction.
 *
 *All commands must terminate with a semicolon.
 *
 *An example of this:
 *To get pin 3 to become an analog output which pwm's at
 *128, we would send the following commands:
 *pm0003OUTP;
 *aw00030128;
 *
 *Motors have a speed range from 0-2000, where 1000 is
 *neutral. 
 *The left motor is pin 2 and the right motor is pin 3
 **********************************************************/

// The left motor is pin 2 and the right motor is pin 3
char right_motor_forward[12] = "pt00031300;";  //length 11
char right_motor_reverse[12] = "pt00030700;";  //length 11
char right_motor_stop[12] = "pt00031000;";     //length 11
char left_motor_forward[12] = "pt00021300;";   //length 11
char left_motor_reverse[12] = "pt00020700;";   //length 11
char left_motor_stop[12] = "pt00021000;";      //length 11
char delay[8] = "dy1000;"; //length 7


void TurnLeft(SimpleSerial* serial) {
  serial->serial_write(left_motor_reverse, 11);
  serial->serial_write(right_motor_forward, 11);
}

void TurnRight(SimpleSerial* serial) {
  serial->serial_write(right_motor_reverse, 11);
  serial->serial_write(left_motor_forward, 11);
}

void Reverse(SimpleSerial* serial) {
  serial->serial_write(left_motor_reverse, 11);
  serial->serial_write(right_motor_reverse, 11);
}

void Forward(SimpleSerial* serial) {
  serial->serial_write(left_motor_forward, 11);
  serial->serial_write(right_motor_forward, 11);
}

void Stop(SimpleSerial* serial) {
  serial->serial_write(right_motor_stop, 11);
  serial->serial_write(left_motor_stop, 11);
  exit(0);
}

void Wait(int seconds) {
  clock_t endwait;
  endwait = clock() + seconds * CLOCKS_PER_SEC;
  while (clock() < endwait) {}
}

int main(int argc, char **argv) {
  string input = "";
  SimpleSerial serial(argv[1], atoi(argv[2]));
  
  while (input != "quit") {
    cin >> input;
    
    if (input == "a")
       TurnLeft(&serial);
    if (input == "d")
       TurnRight(&serial);
    if (input == "w")
       Forward(&serial);
    if (input == "s")
       Reverse(&serial);
    if (input == "e")
       Stop(&serial);
    if (input == "q") {
       Stop(&serial);
       exit(0);
    }
    Wait(1);
  } 
     
}

